#include "AutomaticCtrl.h"
#include <cob_object_detection_msgs/Detection.h>
//(*InternalHeaders(AutomaticCtrl)
#include <wx/string.h>
#include <wx/intl.h>
//*)

//(*IdInit(AutomaticCtrl)
const long AutomaticCtrl::ID_BUTTON1 = wxNewId();
const long AutomaticCtrl::ID_BUTTON4 = wxNewId();
const long AutomaticCtrl::ID_BUTTON5 = wxNewId();
const long AutomaticCtrl::ID_CHOICE1 = wxNewId();
const long AutomaticCtrl::ID_CHOICE2 = wxNewId();
//*)

BEGIN_EVENT_TABLE(AutomaticCtrl,wxFrame)
	//(*EventTable(AutomaticCtrl)
	//*)
END_EVENT_TABLE()

AutomaticCtrl::AutomaticCtrl(wxWindow* parent, RosInterface *Ri, ProcessManager *PM, wxWindowID id,const wxPoint& pos,const wxSize& size)
{
    this->Ri = Ri;
    this->PM = PM;

	//(*Initialize(AutomaticCtrl)
	Create(parent, wxID_ANY, wxEmptyString, wxDefaultPosition, wxDefaultSize, wxDEFAULT_FRAME_STYLE, _T("wxID_ANY"));
	SetClientSize(wxSize(230,142));
	Button1 = new wxButton(this, ID_BUTTON1, _("Detect objects"), wxPoint(8,16), wxSize(-1,-1), 0, wxDefaultValidator, _T("ID_BUTTON1"));
	Button2 = new wxButton(this, ID_BUTTON4, _("Simulate grasp"), wxPoint(8,64), wxSize(113,29), 0, wxDefaultValidator, _T("ID_BUTTON4"));
	Button3 = new wxButton(this, ID_BUTTON5, _("Grasp object"), wxPoint(8,96), wxSize(113,29), 0, wxDefaultValidator, _T("ID_BUTTON5"));
	det_desplegable = new wxChoice(this, ID_CHOICE1, wxPoint(144,16), wxDefaultSize, 0, 0, 0, wxDefaultValidator, _T("ID_CHOICE1"));
	grasp_desplegable = new wxChoice(this, ID_CHOICE2, wxPoint(144,80), wxDefaultSize, 0, 0, 0, wxDefaultValidator, _T("ID_CHOICE2"));

	Connect(ID_BUTTON1,wxEVT_COMMAND_BUTTON_CLICKED,(wxObjectEventFunction)&AutomaticCtrl::DetectObjects);
	Connect(ID_BUTTON4,wxEVT_COMMAND_BUTTON_CLICKED,(wxObjectEventFunction)&AutomaticCtrl::simulateGrasp);
	Connect(ID_BUTTON5,wxEVT_COMMAND_BUTTON_CLICKED,(wxObjectEventFunction)&AutomaticCtrl::graspObject);
	Connect(ID_CHOICE1,wxEVT_COMMAND_CHOICE_SELECTED,(wxObjectEventFunction)&AutomaticCtrl::getGrasps);
	//*)
}

AutomaticCtrl::~AutomaticCtrl()
{
	//(*Destroy(AutomaticCtrl)
	//*)
}


void AutomaticCtrl::DetectObjects(wxCommandEvent& event)
{
    detected_objects = (Ri->Detect()).detections;
    /*
Header header
string label
string detector
float32 score
Mask mask
geometry_msgs/PoseStamped pose
*/
    det_desplegable->Clear();
    std::vector<std::string> object_names;
    for (int i=0; i<detected_objects.size(); i++)
    {
        object_names.push_back(detected_objects[i].label);
        det_desplegable->Append(wxString(detected_objects[i].label.c_str(), wxConvUTF8).c_str());
    }
}

void AutomaticCtrl::getGrasps(wxCommandEvent& event)
{
    //TODO: obtener id a partir del nombre
    //int object_id = getIdFromName(detected_objects[event.GetSelection()].label.c_str());

    int object_id_fake = 1;
    geometry_msgs::Pose p = Ri->transformPose(detected_objects[event.GetSelection()].pose);
    grasp_configurations = Ri->getGraspsFromPosition(object_id_fake, p);

    grasp_desplegable->Clear();
    for (int i=0; i<grasp_configurations.size(); i++)
    {
        grasp_desplegable->Append(wxString::Format(wxT("%i"),i));
    }
}

void AutomaticCtrl::simulateGrasp(wxCommandEvent& event)
{
    int object_id_fake = 1;
    Ri->simulate_grasp(object_id_fake, grasp_configurations[grasp_desplegable->GetSelection()], Ri->transformPose(detected_objects[det_desplegable->GetSelection()].pose));
}

void AutomaticCtrl::graspObject(wxCommandEvent& event)
{
    srs_msgs::GraspSubConfiguration gs = grasp_configurations[grasp_desplegable->GetSelection()];
    geometry_msgs::PoseStamped pre, g;
    pre.header.frame_id = "/base_link";
    pre.pose = gs.pre_grasp;
    g.header.frame_id = "/base_link";
    g.pose = gs.grasp;
    std::vector<float> pre_grasp_conf, grasp_conf;
    std::vector<float> current_joint_configuration;
    current_joint_configuration.resize(7);
    /*
    bool found = false;
    for (int i=0; i<5; i++)
    {
        pre_grasp_conf = Ri->callIkSolver(current_joint_configuration, pre);
        ROS_ERROR("i: %d", i);
        if (pre_grasp_conf.size() > 0)
        {
            for (int j=0; j<5; j++)
            {
                ROS_ERROR("j: %d", j);
                grasp_conf = Ri->callIkSolver(pre_grasp_conf, g);
                if (grasp_conf.size() > 0)
                {
                    found = true;
                    break;
                }
            }
            if (found==true)
            {
                Ri->move("arm", pre_grasp_conf);
                break;
            }
        }
    }
    */


    for (int i=0; i<5; i++)
    {
        ROS_ERROR("i: %d", i);
        if (Ri->computeIK(current_joint_configuration, pre, pre_grasp_conf))
        {
            for (int j=0; j<5; j++)
            {
                ROS_ERROR("j: %d", j);
                if (Ri->callIkSolver(pre_grasp_conf, g, grasp_conf))
                {
                    found = true;
                    break;
                }
            }
            if (found==true)
            {
                ROS_ERRORS("ENCONTRADAAAAAAAAAAAAAA");
                Ri->move("arm", pre_grasp_conf);
                break;
            }
        }
    }



}
